/*!
  @file   ObjectIdentifier.h
  @brief  Identify object inside marker
  @author Kyohei Otsu

  subscribe squared inner image. detect color and shape of the object
*/

#include <vector>

#include <ros/ros.h>
#include <nodelet/nodelet.h>
#include <opencv2/ml/ml.hpp>
#include <sensor_msgs/image_encodings.h>

#include "camera/MarkerMetaData.h" 
#include "camera/MarkerImage.h"

#define FOR(x, a, b) for(typeof(a) x = (a); x < static_cast<typeof(a)>(b); x++)
#define FORIT(x, v) for(typeof(v.begin()) x = v.begin(); x != v.end(); x++)

using namespace std;

typedef std::vector<float> Feature_t;

namespace marker_detector
{

const int MARKER_SIZE = 50;

//! @brief  Object identifier class
class ObjectIdentifier : public nodelet::Nodelet
{

private:

  //! pub/sub
  ros::Subscriber marker_sub_;
  ros::Publisher marker_meta_pub_;

  //! SVM
  CvSVM svm_;
  std::vector<std::string> label_;

  //! constant values
  enum { K, B, G, M, }; //!< color

public:

  //! @brief  initialization for nodelet manager
  virtual void onInit();

  //! @brief  callback of marker image
  void imageCallback(const camera::MarkerImageConstPtr &msg);

  //! @brief  identification
  //! @param  marker squared inner image
  //! @param  meta result of object identification
  int identify_object(cv::Mat &marker, camera::MarkerMetaDataPtr &meta);

protected:
  //! @brief  training
  //! @note   training_from_rosbag() not implemented
  void training_from_pngdata();
  void training_from_rosbag();

  //! extract color from marker
  int extract_color(cv::Mat &bgr, cv::Mat &mono);

  //! @brief  extract bag of features from img
  //! @param  m original image
  //! @param  f output vector
  void extract_feature(cv::Mat& m, Feature_t& f);

 private:
  //! @brief  make kernel for gabor filter
  cv::Mat _mk_kernel(int, double, double, double, double);

  //! @brief  compute distance between features a & b
  float _get_dist(Feature_t& a, Feature_t& b);

  //! @brief  estimate color from voting
  int _get_color(cv::Mat &img);

};

} // end of namespace
